package ags.muse.recon;

import ags.util.points.*;

public class RobotHistory {
	// Whether the tick is estimated or real
	private boolean estimated;
    public boolean isEstimated() { return estimated; }
	
	// Snapshot data
	private AbsolutePoint location;
	private RelativePoint velocity;
	private double energy;
	private final long time;
	private final int orientation;

	// Snapshot Getters
	public AbsolutePoint getLocation() { return location; }
	public RelativePoint getVelocity() { return velocity; }
	public double getEnergy() { return energy; }
	public long getTime() { return time; }
	public int getOrientation() { return orientation; }

	// Linked List
	private final RobotHistory prev;
	private RobotHistory next;
	private final RobotHistory first;
	private final RobotHistory prevReal;
	private RobotHistory nextReal;

	// Linked List Getters
	public RobotHistory prev() { return prev; }
	public RobotHistory next() { return next; }
	public RobotHistory first() { return first; }
	public RobotHistory prevReal() { return prevReal; }
	public RobotHistory nextReal() { return nextReal; }

	public RobotHistory(AbsolutePoint location, RelativePoint velocity, double energy, long time, RobotHistory prev, RobotHistory prevReal) {
		this.location = location;
		this.velocity = velocity;
		this.energy = energy;
		this.time = time;
		this.prev = prev;
		if (prev != null) {
			this.first = prev.first;
			prev.next = this;
		} else {
			this.first = this;
		}
		this.prevReal = prevReal;
		if (prevReal != null) {
			prevReal.nextReal = this;
		}
		if (velocity.magnitude != 0) {
			this.orientation = velocity.magnitude > 0 ? 1 : -1;
		} else if (prev != null) {
			this.orientation = prev.orientation;
		} else {
			this.orientation = 1;
		}
	}

	public RobotHistory update(AbsolutePoint newLocation, RelativePoint newVelocity, double newEnergy, long newTime) {
	    if (energy == 0) {
	        // Disabled, begin a new history
	        return new RobotHistory(newLocation, newVelocity, newEnergy, newTime, null, null);
	    }
	    else if (newTime == time + 1) {
			// Next tick, just add it
			return new RobotHistory(newLocation, newVelocity, newEnergy, newTime, this, this);
		}
		else if (newTime > time) {
			// Linearly interpolate location/velocity/energy/time
			RobotHistory cursor = this;
			double dlx = (newLocation.x - location.x) / (newTime - time);
			double dly = (newLocation.y - location.y) / (newTime - time);
			double dvx = (newVelocity.x - velocity.x) / (newTime - time);
			double dvy = (newVelocity.y - velocity.y) / (newTime - time);
			double de = (newEnergy - energy) / (newTime - time);
			for (long t = time + 1; t < newTime; t++) {
				double lx = location.x + dlx * (t - time);
				double ly = location.y + dly * (t - time);
				double vx = velocity.x + dvx * (t - time);
				double vy = velocity.y + dvy * (t - time);
				double e = energy + de * (t - time);
				AbsolutePoint loc = AbsolutePoint.fromXY(lx, ly);
				RelativePoint vel = RelativePoint.fromXY(vx, vy);
				cursor = new RobotHistory(loc, vel, e, t, cursor, null);
				cursor.estimated = true;
			}
			
			return new RobotHistory(newLocation, newVelocity, newEnergy, newTime, cursor, this);
		}

		else if (newTime < time) {
			// Old data, do nothing
			return this;
		}
		else if (newTime == time && !estimated) {
			// Repeat data, do nothing
			return this;
		}
		else if (newTime == time && estimated) {
			// Was estimated, replace estimated data
			location = newLocation;
			velocity = newVelocity;
			energy  = newEnergy;
			estimated = false;
			return this;
		}
		else {
			return this;
		}
	}
}
